﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Diagnostics;
using System.Drawing;
using System.IO;
using System.Text;
using System.Threading;
using System.Windows.Forms;
using System.Windows.Forms.DataVisualization.Charting;
using System.Linq;

struct Frame
{
  public Byte TrID;
  public float X;
  public float Y;
  static public int Length = 9;
}

namespace UHSonar
{
  public partial class MainForm : Form
  {
    int chartZoom = 1;

    private readonly double sizeX = 1.4;
    private readonly double sizeY = 2;

    Stopwatch mStopper = new Stopwatch();

    public MainForm()
    {
      InitializeComponent();
      serialPort.ReceivedBytesThreshold = Frame.Length;
      serialPort.PortName = "COM4";

      camSerialPort.PortName = "COM4";

      chart.Series[0].Points.AddXY(0.6, 2);

      LabelStyle style = new LabelStyle();
      style.Format = "0.##";

      chart.ChartAreas[0].AxisX.MajorGrid.Interval = 0.1;
      chart.ChartAreas[0].AxisX.LabelStyle.Interval = 0.5;
      chart.ChartAreas[0].AxisY.LabelStyle = style;
      chart.ChartAreas[0].AxisY.MajorGrid.Interval = 0.1;
      chart.ChartAreas[0].AxisY.LabelStyle.Interval = 0.5;

      chart.ChartAreas[0].AxisX.Maximum = sizeX;
      chart.ChartAreas[0].AxisY.Maximum = sizeY;
    }

    private void serialPort_DataReceived(object sender, System.IO.Ports.SerialDataReceivedEventArgs e)
    {
      Byte[] data = new Byte[Frame.Length];
      serialPort.Read(data, 0, Frame.Length);
      Frame frame = new Frame();
      frame.TrID = data[0];
      frame.X = BitConverter.ToSingle(data, 1);
      frame.Y = BitConverter.ToSingle(data, 5);
      chart.Series[frame.TrID].Points.AddXY(frame.Y, frame.X);

      String txt = String.Format("X: {0,3:N3}\nY: {1,3:N3}", frame.X, frame.Y);
      switch (frame.TrID)
      {
        case (0): Tr1txt.Text = txt; break;
        case (1): Tr2txt.Text = txt; break;
        case (2): Tr3txt.Text = txt; break;
        case (3): Tr4txt.Text = txt; break;
      }
    }

    private void camSerialPort_DataReceived(object sender, System.IO.Ports.SerialDataReceivedEventArgs e)
    {
      Light[] lights = new Light[3];
      string text = "";
      byte[] data = new byte[1000];

      while (camSerialPort.ReadByte() != 0x5a) ;
      int num = camSerialPort.ReadByte();
      while (camSerialPort.BytesToRead < num * 12) ;
      camSerialPort.Read(data, 0, num * 12);
      text += num + "\t" + mStopper.ElapsedMilliseconds + "\n\r";
      mStopper.Restart();
      for (int i = 0; i < num; i++)
      {
        float angle = BitConverter.ToSingle(data, i * 12);
        float radius = BitConverter.ToSingle(data, i * 12 + 4);
        float area = BitConverter.ToSingle(data, i * 12 + 8);

        if (i < 3)
        {
          lights[i].Angle = angle;
          lights[i].Radius = radius;
          lights[i].Area = area;
        }

        text += string.Format("P{3}: Radius:{1,3:N2} Angle={0,2:N4} Area:{2}\n", angle, radius, area, i);
      }

      text += "-------------------------------------------------------\n";
      
      if (num == 3)
      {
        lights = lights.OrderBy(i => i.Angle).ToArray();

        text += "\n";

        // calculatin position
        double w = sizeY;
        double h = sizeX;

        for (int i = 0; i < 3; i++)
        {
          double alfa = lights[(i + 1) % 3].Angle - lights[i].Angle;
          if (i == 2) alfa += Math.PI * 2;

          double beta = lights[(i + 2) % 3].Angle - lights[(i + 1) % 3].Angle;
          if (i == 1) beta += Math.PI * 2;

          double v1 = Math.Cos(alfa) * Math.Cos(alfa) * Math.Sin(beta) * Math.Sin(beta);
          double v2 = Math.Sin(alfa) * Math.Sin(alfa) * Math.Cos(beta) * Math.Cos(beta);
          double v3 = Math.Sin(alfa) * Math.Cos(alfa);
          double v4 = Math.Sin(beta) * Math.Cos(beta);

          double nom_x = 4 * h * h * w * v1 + h * v3 * (4 * h * w * v4 + (h * h - 4 * w * w) * Math.Sin(beta) * Math.Sin(beta) - 4 * w * w) + (h * h - 4 * w * w) * Math.Sin(alfa) * Math.Sin(alfa) * (h * v4 - w);
          double den = 4 * h * h * v1 + 4 * h * v3 * Math.Sin(beta) * (h * Math.Cos(beta) - 2 * w * Math.Sin(beta)) - Math.Sin(alfa) * Math.Sin(alfa) * (8 * h * w * v4 - h * h - 4 * w * w);
          double nom_y = h * (h * h * v1 + v3 * ((h * h + 4 * w * w) * v4 - 2 * h * w) + 4 * w * w * v2 - h * Math.Sin(beta) * (4 * w * Math.Cos(beta) - h * Math.Sin(beta)));

          PointD position = new PointD(w - nom_x / den, nom_y / den);

          // calculatin orientation
          double gamma = lights[(i + 2) % 3].Angle;
          double gamma0 = Math.PI / 2 - Math.Atan((sizeX / 2 - position.X) / (sizeY - position.Y));

          double orientation = gamma - gamma0;

          if (position.X > 0 && position.X < w && position.Y > 0 && position.Y < h)
          {
            text += String.Format(" Pos: \t ({0,3:N4}, {1, 3:N4}),\t Orientation: {2, 3:N4}\n", position.X, position.Y, orientation);
          }
        }

        // calculating center
        double c_x = 360;
        double c_y = 230;

        PointD A = new PointD(c_x - lights[0].Radius * Math.Sin(lights[0].Angle), c_y - lights[0].Radius * Math.Cos(lights[0].Angle));
        PointD B = new PointD(c_x - lights[1].Radius * Math.Sin(lights[1].Angle), c_y - lights[1].Radius * Math.Cos(lights[1].Angle));
        PointD C = new PointD(c_x - lights[2].Radius * Math.Sin(lights[2].Angle), c_y - lights[2].Radius * Math.Cos(lights[2].Angle));

        double D = 2 * (A.X * (B.Y - C.Y) + B.X * (C.Y - A.Y) + C.X * (A.Y - B.Y));

        PointD O = new PointD
        {
          X = ((A.X * A.X + A.Y * A.Y) * (B.Y - C.Y) + (B.X * B.X + B.Y * B.Y) * (C.Y - A.Y) + (C.X * C.X + C.Y * C.Y) * (A.Y - B.Y)) / D,
          Y = ((A.X * A.X + A.Y * A.Y) * (C.X - B.X) + (B.X * B.X + B.Y * B.Y) * (A.X - C.X) + (C.X * C.X + C.Y * C.Y) * (B.X - A.X)) / D
        };

        text += String.Format(" Center: \t ({0,3:N2}, {1,3:N2})\n", O.X, O.Y);

      }
      try
      {
        Invoke(new Action(() => CamTxt.Text = text));
      }
      catch { }
    }

    private void chart_MouseClick(object sender, MouseEventArgs e)
    {
      double xmax = chart.ChartAreas[0].AxisX.Maximum;
      double xmin = chart.ChartAreas[0].AxisX.Minimum;
      double ymax = chart.ChartAreas[0].AxisY.Maximum;
      double ymin = chart.ChartAreas[0].AxisY.Minimum;

      double x;
      double y;

      if (e.X < 45) x = 0.01;
      else if (e.X > 375) x = sizeX - 0.01;
      else x = xmin + (xmax - xmin) * ((e.X - 45) / 330.0);

      if (e.Y < 28) y = sizeY - 0.01;
      else if (e.Y > 485) y = 0.01;
      else y = ymax - (ymax - ymin) * ((e.Y - 28) / 457.0);

      if (e.Button == MouseButtons.Left) chartZoom *= 2;
      else if ((e.Button == MouseButtons.Right) && chartZoom > 1) chartZoom /= 2;

      chart.ChartAreas[0].AxisY.LabelStyle.Interval = 0.5 / chartZoom;
      chart.ChartAreas[0].AxisX.LabelStyle.Interval = 0.5 / chartZoom;

      double xArea = sizeX / chartZoom;
      double yArea = sizeY / chartZoom;

      if ((x > xArea / 2) && (x < (sizeX - xArea / 2)) && (y > yArea / 2) && (y < (sizeY - yArea / 2)))
      {
        chart.ChartAreas[0].AxisY.Maximum = y + yArea / 2;
        chart.ChartAreas[0].AxisY.Minimum = y - yArea / 2;

        chart.ChartAreas[0].AxisX.Maximum = x + xArea / 2;
        chart.ChartAreas[0].AxisX.Minimum = x - xArea / 2;
      }
      else
      {
        chart.ChartAreas[0].AxisY.Maximum = ((int)(y / yArea) + 1) * yArea;
        chart.ChartAreas[0].AxisY.Minimum = (int)(y / yArea) * yArea;

        chart.ChartAreas[0].AxisX.Maximum = ((int)(x / xArea) + 1) * xArea; ;
        chart.ChartAreas[0].AxisX.Minimum = (int)(x / xArea) * xArea;
      }

      chart.Invalidate();
    }

    private void btnConn_Click(object sender, EventArgs e)
    {
      if (btnConn.Text == "UH Connect")
      {
        try
        {
          serialPort.Open();
          btnConn.Text = "UH Disconnect";
        }
        catch (Exception ex) { MessageBox.Show(ex.Message); }
      }
      else
      {
        serialPort.Close();
        btnConn.Text = "UH Connect";
      }
    }    

    private void btnConnCam_Click(object sender, EventArgs e)
    {
      if (btnConnCam.Text == "CAM Connect")
      {
        try
        {
          camSerialPort.Open();
          btnConnCam.Text = "CAM Disconnect";
        }
        catch (Exception ex) { MessageBox.Show(ex.Message); }
      }
      else
      {
        camSerialPort.Close();
        btnConnCam.Text = "CAM Connect";
      }
    }

    private void btnSave_Click(object sender, EventArgs e)
    {

    }
  }

  public struct Light
  {
    public double Angle { set; get; }
    public double Radius { set; get; }
    public double Area { set; get; }
  }

  public struct PointD
  {
    public PointD(double x, double y)
      : this()
    {
      this.X = x;
      this.Y = y;
    }

    public double X { set; get; }
    public double Y { set; get; }
  }
}
